//
// Created by Pulsar on 2020/5/15.
//
#include <rc_gpio/c_gpio.h>
#include <rc_gpio/soft_pwm.h>
#include <string>
#include <iostream>
#include <cstdio>

int PWM_1 = 1;
int PWM_2 = 0;
int PWM_3 = 3;

int DIST_1 = 405;
int DIST_2 = 402;
int DIST_3 = 431;

void init_pwm() {
    RC::GPIO::pwm_begin(PWM_1);
    RC::GPIO::pwm_set_frequency(PWM_1, 3413333);
    RC::GPIO::pwm_set_duty_cycle(PWM_1, 1706667);
    RC::GPIO::pwm_start(PWM_1);

    RC::GPIO::pwm_begin(PWM_2);
    RC::GPIO::pwm_set_frequency(PWM_2, 3413333);
    RC::GPIO::pwm_set_duty_cycle(PWM_2, 1706667);
    RC::GPIO::pwm_start(PWM_2);

    RC::GPIO::pwm_begin(PWM_3);
    RC::GPIO::pwm_set_frequency(PWM_3, 3413333);
    RC::GPIO::pwm_set_duty_cycle(PWM_3, 1706667);
    RC::GPIO::pwm_start(PWM_3);
}

void init_direction() {
    RC::GPIO::pinMode(DIST_1, RC_GPIO_OUTPUT);
    RC::GPIO::digitalWrite(DIST_1, RC_GPIO_LOW);

    RC::GPIO::pinMode(DIST_2, RC_GPIO_OUTPUT);
    RC::GPIO::digitalWrite(DIST_2, RC_GPIO_LOW);

    RC::GPIO::pinMode(DIST_3, RC_GPIO_OUTPUT);
    RC::GPIO::digitalWrite(DIST_3, RC_GPIO_LOW);
}

void release_all() {
    RC::GPIO::pinFree(DIST_1);
    RC::GPIO::pinFree(DIST_2);
    RC::GPIO::pinFree(DIST_3);

    RC::GPIO::pwm_stop(PWM_1);
    RC::GPIO::pwm_free(PWM_1);

    RC::GPIO::pwm_stop(PWM_2);
    RC::GPIO::pwm_free(PWM_2);

    RC::GPIO::pwm_stop(PWM_3);
    RC::GPIO::pwm_free(PWM_3);

}

void speed_change(int freq) {
    RC::GPIO::pwm_set_frequency(PWM_1, freq);
    RC::GPIO::pwm_set_frequency(PWM_2, freq);
    RC::GPIO::pwm_set_frequency(PWM_3, freq);
}

void direction_change(bool dist) {
    if (dist) {
        RC::GPIO::digitalWrite(DIST_1, RC_GPIO_LOW);
        RC::GPIO::digitalWrite(DIST_2, RC_GPIO_LOW);
        RC::GPIO::digitalWrite(DIST_3, RC_GPIO_LOW);
    } else {
        RC::GPIO::digitalWrite(DIST_1, RC_GPIO_HIGH);
        RC::GPIO::digitalWrite(DIST_2, RC_GPIO_HIGH);
        RC::GPIO::digitalWrite(DIST_3, RC_GPIO_HIGH);
    }
}

int main(int argc, char **argv) {
    char exit_c = 'c';
    int start_freq = 1993333;
    bool dist = false;
    init_pwm();
    init_direction();
    while (exit_c != getchar()) {
//        speed_change(start_freq);
        direction_change(dist);
//        start_freq -= 1000;
        dist = !dist;
        std::cout << "Speed: " << start_freq << std::endl;
    }
    release_all();
    return 0;
}
